/*
BluetoothBee Demo Code - Delay Based Implementaion
2011 Copyright (c) Seeed Technology Inc.  All right reserved.
 
Author: Visweswara R
 
This demo code is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
 
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
Lesser General Public License for more details.
 
You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 
For more details about the product please check http://www.seeedstudio.com/depot/
 
*/
 
/* Upload this sketch into Seeeduino and press reset*/
 
#include <SoftwareSerial.h>   //Software Serial Port
#include <Servo.h> 
#define RxD 11
#define TxD 12
//-----------------GPS------------------------

#include <Wire.h>

#define GPM 0x68  	                              // GPM I2C Address

byte Address;                                         // Global variable
byte Data;                                            // Global variable

//-----------------Distance minimal d'arret-------------------------------
#define DISTANCEMIN 60

//-------------ETAT DU ROBOT-----------------------
#define AVANCE 1
#define RECULE 2
#define TOURNELUIMEME 3
#define ARRETE 0

//---------------IR--------------------
int distAvant = 0;
int distArriere = 0;
int IRpinAvant=2;
int IRpinArriere=3;

Servo servo_virage;
Servo servo_speed;
//SoftwareSerial blueToothSerial(RxD,TxD);
 
int etat=ARRETE; //0 : arret  , 1: avance  , 2: recule  ,   3:tourne sur lui même
 
void setup() 
{ 
    servo_virage.attach(4);
    servo_speed.attach(7);
    Serial.begin(9600);          //Serial port for debugging, Comment this line if not required  
    //pinMode(RxD, INPUT);
    //pinMode(TxD, OUTPUT);
    setupBlueToothConnection();
    Wire.begin();                                       // Start I2C comms
    Serial.begin(9600);                                 // Start PC serial port @ 9600 baud
    delay(100);
    Serial.println("Application started...awaiting GPS data..");

  delay(3000);      
 
} 
 
void loop() 
{ 
  distAvant=calculDistanceAvant(IRpinAvant);
  distArriere=calculDistanceArriere(IRpinArriere);
  
  
 char cmd = Serial1.read();
  switch(cmd){
    case 'z': avancerToutDroit();break;
    case 'd': tourner_droite_arret();break;
    case 's': reculerToutDroit();break;
    case 'q': tourner_gauche_arret();break;
    case 'e': arret();
  }
  
  if((distAvant <= DISTANCEMIN || distArriere <= DISTANCEMIN) && etat==TOURNELUIMEME)
  {
    arret();
  }else if(distAvant <= DISTANCEMIN && etat ==AVANCE){
    arret();
  }else if(distArriere <= DISTANCEMIN && etat==RECULE){
    arret();
  }
  
for(int x=0; x <=2; x++){

  Serial.print("Time: ");
  Address = 0;                                        // Point to Hours register
  Data = GetDouble();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Serial.print(":");
  Address = 2;                                        // Point to Minutes register
  Data = GetDouble();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Serial.print(":");
  Address = 4;                                        // Point to Seconds register
  Data = GetDouble();                                 // Read registers from GPM
  Serial.println(Data, DEC);                          // and print to PC

  Serial.print("Date: ");
  Address = 6;                                        // Point to Day register
  Data = GetDouble();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Serial.print("/");
  Address = 8;                                        // Point to Month register
  Data = GetDouble();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Serial.print("/");
  Address = 10;                                       // Point to Year 1 register
  Data = GetDouble();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Address = 12;                                       // Point to Year 2 register
  Data = GetDouble();                                 // Read registers from GPM
  Serial.println(Data, DEC);                          // and print to PC
  delay(1000);                                        // Wait a second
  }

  Serial.print("Heading: ");
  Address = 44;                                       // Point to Heading 1 register
  Data = GetDouble();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Address = 46;                                       // Point to Heading 2 register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Serial.print(".");
  Address = 47;                                       // Point to Heading 3 register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.println(Data, DEC);                          // and print to PC

  Serial.print("Speed: ");
  Address = 52;                                       // Point to Speed 1 register
  Data = GetDouble();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Address = 54;                                       // Point to Speed 2 register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Serial.print(".");
  Address = 55;                                       // Point to Speed 3 register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.println(Data, DEC);                          // and print to PC

  delay(3000);                                        // Wait 3 seconds

  Serial.print("Latitide: ");
  Address = 14;                                       // Point to Latitude 1 register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Address = 15;                                       // Point to Latitude 2 register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Serial.print(" ");
  Address = 16;                                       // Point to Latitude 3 register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Address = 17;                                       // Point to Latitude 4 register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Serial.print(".");
  Address = 18;                                       // Point to Latitude 5 register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Address = 19;                                       // Point to Latitude 6 register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Address = 20;                                       // Point to Latitude 7 register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Address = 21;                                       // Point to Latitude 8 register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Address = 22;                                       // Point to Latitude character register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.println(Data, DEC);                         // and print to PC

  Serial.print("Longitude: ");
  Address = 23;                                       // Point to Longitude 1 register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Address = 24;                                       // Point to Longitude 2 register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Address = 25;                                       // Point to Longitude 3 register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Serial.print(" ");
  Address = 26;                                       // Point to Longitude 4 register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Address = 27;                                       // Point to Longitude 5 register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Serial.print(".");
  Address = 28;                                       // Point to Longitude 6 register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Address = 29;                                       // Point to Longitude 7 register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Address = 30;                                       // Point to Longitude 8 register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Address = 31;                                       // Point to Longitude 9 register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.print(Data, DEC);                            // and print to PC
  Address = 32;                                       // Point to Longitude character register
  Data = GetSingle();                                 // Read registers from GPM
  Serial.println(Data, DEC);                         // and print to PC

  delay(3000);                                        // Wait 3 seconds

} 
 
void setupBlueToothConnection()
{
    Serial.print("Setting up Bluetooth link");       //For debugging, Comment this line if not required    
    Serial1.begin(38400); //Set BluetoothBee BaudRate to default baud rate 38400
    delay(1000);
    sendBlueToothCommand("\r\n+STWMOD=0\r\n");
    sendBlueToothCommand("\r\n+STNA=Keriosity\r\n");
    sendBlueToothCommand("\r\n+STAUTO=0\r\n");
    sendBlueToothCommand("\r\n+STOAUT=1\r\n");
    sendBlueToothCommand("\r\n+STPIN=0000\r\n");
    delay(2000); // This delay is required.
    Serial1.print("\r\n+INQ=1\r\n");
    delay(2000); // This delay is required.
    Serial.print("Setup complete");
 
}
 
void sendBlueToothCommand(char command[])
{
    char a;
    Serial1.print(command);
    Serial.print(command);                          //For debugging, Comment this line if not required    
    delay(3000);
 
    while(Serial1.available())              //For debugging, Comment this line if not required  
    {                                               //For debugging, Comment this line if not required   
       Serial.print(char(Serial1.read()));  //For debugging, Comment this line if not required  
    }                                               //For debugging, Comment this line if not required   
}


void avancerToutDroit()
{
  int vitesse=110; // 90 --> arret, <90 et >= 45 --> reculer, >90 et <= 135 --> avancer
  int angle=90;    // 90 --> tout droit, >=45 et <90 --> virage droite, >90 et <=135 --> virage gauchee
  // pour tourner sur lui même, vitesse =90 (arret). Et donner une valeur d'angle pour la vitesse de rotation
  
  servo_virage.write(angle);  
  servo_speed.write(vitesse); 
  etat=AVANCE;
}

void reculerToutDroit()
{
  int vitesse=60; // 90 --> arret, <90 et >= 45 --> reculer, >90 et <= 135 --> avancer
  int angle=90;    // 90 --> tout droit, >=45 et <90 --> virage droite, >90 et <=135 --> virage gauchee
  // pour tourner sur lui même, vitesse =90 (arret). Et donner une valeur d'angle pour la vitesse de rotation
  
  servo_virage.write(angle);  
  servo_speed.write(vitesse);  
  etat=RECULE; 
}

void tourner_gauche_arret()
{
  int vitesse=90; // 90 --> arret, <90 et >= 45 --> reculer, >90 et <= 135 --> avancer
  int angle=45;    // 90 --> tout droit, >=45 et <90 --> virage droite, >90 et <=135 --> virage gauchee
  // pour tourner sur lui même, vitesse =90 (arret). Et donner une valeur d'angle pour la vitesse de rotation
  
  servo_virage.write(angle);  
  servo_speed.write(vitesse);
  etat=TOURNELUIMEME; 
}

void tourner_droite_arret()
{
  int vitesse=90; // 90 --> arret, <90 et >= 45 --> reculer, >90 et <= 135 --> avancer
  int angle=135;    // 90 --> tout droit, >=45 et <90 --> virage droite, >90 et <=135 --> virage gauchee
  // pour tourner sur lui même, vitesse =90 (arret). Et donner une valeur d'angle pour la vitesse de rotation
  
  servo_virage.write(angle);  
  servo_speed.write(vitesse);
  etat=TOURNELUIMEME;
}

void arret()
{
  int vitesse=90; // 90 --> arret, <90 et >= 45 --> reculer, >90 et <= 135 --> avancer
  int angle=90;    // 90 --> tout droit, >=45 et <90 --> virage droite, >90 et <=135 --> virage gauchee
  // pour tourner sur lui même, vitesse =90 (arret). Et donner une valeur d'angle pour la vitesse de rotation
  
  servo_virage.write(angle);  
  servo_speed.write(vitesse);
  etat=ARRETE;
}


//---------------------Infra Rouge---------------------------------

int calculDistanceAvant(int IRpin) { //on fait la moyenne de 10 mesures pour lisser les pics
  float distAvant = 0;
  for (int i = 0; i < 10; i++) {
   float volts = analogRead(IRpin)*0.0048828125;   // valeur du télémètre * (5/1024) - si alimentaiton en 3.3.volts changer 5 en 3.3
   distAvant = distAvant + 65*pow(volts, -1.10);
  }
  distAvant = distAvant/10;
  if (distAvant >150 or distAvant < 20) { // si hors limites on met à 150
    distAvant = 150;
  }
  return(distAvant);
}

int calculDistanceArriere(int IRpin) { //on fait la moyenne de 10 mesures pour lisser les pics
  float distance = 0;
  for (int i = 0; i < 10; i++) {
   float volts = analogRead(IRpin)*0.0048828125;   // valeur du télémètre * (5/1024) - si alimentaiton en 3.3.volts changer 5 en 3.3
   distance = distance + 65*pow(volts, -1.10);
  }
  distance = distance/10;
  if (distance >150 or distance < 20) { // si hors limites on met à 150
    distance = 150;
  }
  return(distance);
}
//------------------------GPS----------------------------------------


int GetDouble(){				      // Get double register value from GPM

  int Value = 0; 
  byte H_Byte = 0;
  byte L_Byte = 0;

  Wire.beginTransmission(GPM);
  Wire.write(Address);				      // Send register start address
  Wire.endTransmission();

  Wire.requestFrom(GPM, 2);			      // Request double register
  while(Wire.available() < 2);			      // Wait for double byte
  H_Byte = Wire.read();                            // Store one
  L_Byte = Wire.read();                            // Store two

  Value = (H_Byte * 10) + L_Byte;                     // Adjust for one byte

  return(Value);	                          
}


int GetSingle(){				      // Get single register value from GPM

  int Value = 0; 

  Wire.beginTransmission(GPM);
  Wire.write(Address);				      // Send register start address
  Wire.endTransmission();

  Wire.requestFrom(GPM, 1);			      // Request register
  while(Wire.available() < 1);			      // Wait for single byte
  Value = Wire.read();                             // and store

  return(Value);	                          
}


